
package environment;

import java.awt.Point;
import java.util.ArrayList;

public class PathCalculator 
{
	static int counter = 0;
	public static ArrayList<Goal> getGoalPath(Box b, Goal finalGoal)
	{
		ArrayList<Goal> goalPath = new ArrayList<Goal>();
		Point curPos = new Point(b.getX(), b.getY());
		
		validatePath(curPos, finalGoal, goalPath, b, true);
		counter++;		
		return goalPath;
	}
	
	public static void validatePath(Point curPos, Goal nextGoal, ArrayList<Goal> goalList, Box b, boolean add)
	{
		Line path = new Line(curPos, new Point(nextGoal.x, nextGoal.y)), newPath;
		double distance = 0, err =10, old = 100000, angle = b.getGoalAngle2(new Point(nextGoal.x, nextGoal.y));
		int maskX = 1 , maskY = 1, newX = 0, newY = 0;
		EnvObjInterface o = null;
		Obstacle o2;
		Box o3;
		Goal newGoal = nextGoal;
		EnvObjInterface t;
		for(EnvObjInterface e: Main.a)
		{
			if(e.getID().startsWith("o") ||  (e.getID().startsWith("b") && !b.getID().equals(e.getID()) && ((Box)e).isPushable() )  )
			{
				if(e.getID().startsWith("o"))
				{
					o2 = (Obstacle) e;
					distance = curPos.distance(new Point(o2.getX(), o2.getY()));
					
					if(path.intersects(o2.getBounds()) && distance < old)
					{
						/** Obstacle in the way, add new goal **/
						o = o2;
						old = distance;
					}
				}
				else
				{
					o3 = (Box) e;
					distance = curPos.distance(new Point(o3.getX(), o3.getY()));
					
					if(path.intersects(o3.getBounds()) && distance < old)
					{
						/** Obstacle in the way, add new goal **/
						o = o3;
						old = distance;
					}
				}
				
				
			}
		}
		
		if(o != null)
		{
			System.out.println(b.getGoalAngle2(new Point(Main.g1.x, Main.g1.y))+ "  --- ANGLE");
			if((!skip(b.getGoalAngle2(new Point(Main.g1.x, Main.g1.y))) && counter == 1 ))
			{	
				System.out.println(skip(b.getGoalAngle2(new Point(Main.g1.x, Main.g1.y))));
				newX = Main.g1.x;
				newY = curPos.y;
				newPath = new Line(curPos, new Point(newX, newY));
				System.out.println("USING X OF GOAL");
				if(newPath.intersects(o.getBounds()))
				{
					newX = curPos.x;
					newY = Main.g1.y - (int)b.getWidth()-15;// - (int)b.getHeight();			
					System.out.println("NOPE USE Y OF GOAL");
				}
				newGoal = new Goal(newX, newY, 1);
			}
			else{
				System.out.println("USE NORMAL PATH PLANNING");
				/** TODO Improve this next goal calculation! **/
				if(angle <= 0 + err || angle >= 360 - err)
				{
					System.err.println("1");
					maskY = 0;
				}
				else if(angle <= 180 + err && angle >= 180 - err)
				{
					System.err.println("2");
					maskY = 0;
				}
				else if(angle <= 270 + err && angle >= 270 - err)
				{
					System.err.println("3");
					maskX = 0;
				}
				else if(angle <= 90 + err && angle >= 90 - err)
				{
					System.err.println("4");
					maskX = 0;
				}
				
				// Shouldnt come here
				else if(angle <= 90 && angle >= 0)
				{
					System.err.println("5");
					maskY = 0;
				}
				else if(angle <= 180 && angle >= 90)
				{
					System.err.println("6");
					maskX = 0;
				}
				else if(angle <= 270 && angle >= 180)
				{
					System.err.println("7");
					maskY = 0;
				}
				else if(angle >= 270)
				{
					System.err.println("8");
					maskY = 0;
				}
				newX = (int) (o.getX()- (maskX*o.getWidth()) - (maskX*b.getWidth()) );
				newY = (int) (o.getY()- (maskY*o.getWidth()) - (maskY*b.getWidth()));
				newGoal = new Goal(newX, newY, 1);
			}						
		}
		
		if(add) goalList.add(0, newGoal);
	}
	
	private static boolean skip(double angle2)
	{	
		return (angle2 >= 350) || (angle2 <= 10) ||  (angle2 >= 80 && angle2 <= 100) ||
		(angle2 >= 170 && angle2 <= 190) || (angle2 >= 260 && angle2 <= 280);
	}
	
}
